%逆解
function L  = Stewart_IK_AlgebraFunc(x,y,z,a,b,g)
%%运动学参数
%零初始位置时静平台位置（相对于静平台坐标系）
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置（相对于动平台坐标系）
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];


%%
L = zeros(6,1);
X=[x;y;z];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
l1=norm(X+R*P1-B1);
l2=norm(X+R*P2-B2);
l3=norm(X+R*P3-B3);
l4=norm(X+R*P4-B4);
l5=norm(X+R*P5-B5);
l6=norm(X+R*P6-B6);
L=[l1;l2;l3;l4;l5;l6];
